#include "gimbal.h"

#include "base_drv/drv_conf.h"
#include "base_drv/motor_ctrl.h"
#include "base_drv/ext_imu.h"
#include "base_drv/rng.h"

#include "algorithm/pid.h"
#include "algorithm/filter.h"
#include "algorithm/kalman.h"
#include "algorithm/imu_fusion.h"

#include "pid_parameters.h"
#include "chassis.h"
#include "fire_control.h"

#include <stdlib.h>
#include <math.h>
#include <string.h>

#include "trice.h"
#define TRICE_FILE Id(47478)

//#define ANGLE_DELTA(A) if(A > ANGLE_RANGE/2) A-=ANGLE_RANGE; \
//									else if(A < -ANGLE_RANGE/2) A+=ANGLE_RANGE;

#define PITCH_COMP 0

#define NOD_HEAD 1
#define HIGH_PITCH 1
#define LOW_PITCH 0
#define MID_PITCH 2
int8_t pitch_state=0;

pid_struct_t pitch_pid[2], yaw_pid[2];
pid_struct_t pitch_imu_pid[2], yaw_imu_pid[2];

leso_para_t pitch_imu_leso,yaw_imu_leso;
kalman1_state i_pitch_filter,i_yaw_filter;

RAM_PERSIST int16_t pitch_ang_ref;
RAM_PERSIST int16_t yaw_ang_ref;

RAM_PERSIST gimbal_state_t gimbal_state;
RAM_PERSIST gimbal_state_t gimbal_last_state;
uint16_t start_time_cnt = 0;
uint8_t gimbal_reset_flag;

uint16_t pitch_state_cnt=0;
#define PITCH_STATE_CNT_MAX 450

int16_t gimabal_inspect_pitch_max =500;
int16_t gimabal_inspect_pitch_min =-200;
float pitch_inspect_speed = 5.0f;
float yaw_inspect_speed = 4.0f;
int8_t pitch_sign_inspect=1;
int8_t yaw_sign_inspect=1;
int ttf_cnt=0;
float write_down_demo_yaw=0, write_down_demo_pitch=0;

float pitch_output, yaw_output;
extern gimbal_command_t gimbal_command_data;
extern CAN_HandleTypeDef GIMBAL_MOTORS_HCAN;
extern rx_bag vision_control_data1;
extern int16_t yaw_err;
HAL_StatusTypeDef gimbal_ctrl(motor_response_msg_t* motor_data_ptr,int16_t raw_pitch_ang, int16_t raw_yaw_ang);
HAL_StatusTypeDef gimbal_imu_ctrl(eular_t* imu_eular, imu_data_fp_t* imu_gyro,int16_t raw_pitch_ang, int16_t raw_yaw_ang);
gimbal_state_t gimbal_smooth_reset(motor_response_msg_t* motor_data_ptr);

uint8_t pitch_state_change(void);

void gimbal_init(uint8_t reset_all_flag){
	
	memcpy(&pitch_pid[ANG_LOOP],&pitch_pid_ang_loop,sizeof(pid_struct_t));
	memcpy(&pitch_pid[RPM_LOOP],&pitch_pid_rpm_loop,sizeof(pid_struct_t));
	
	memcpy(&yaw_pid[ANG_LOOP],&yaw_pid_ang_loop,sizeof(pid_struct_t));
	memcpy(&yaw_pid[RPM_LOOP],&yaw_pid_rpm_loop,sizeof(pid_struct_t));
	
	memcpy(&pitch_imu_pid[ANG_LOOP],&pitch_imu_pid_ang_loop,sizeof(pid_struct_t));
	memcpy(&pitch_imu_pid[RPM_LOOP],&pitch_imu_pid_rpm_loop,sizeof(pid_struct_t));
	
	memcpy(&yaw_imu_pid[ANG_LOOP],&yaw_imu_pid_ang_loop,sizeof(pid_struct_t));
	memcpy(&yaw_imu_pid[RPM_LOOP],&yaw_imu_pid_rpm_loop,sizeof(pid_struct_t));

	if(reset_all_flag){
		pitch_ang_ref = 0;
		yaw_ang_ref = GIMBAL_YAW_CENTER;
		start_time_cnt = 0;
		gimbal_state = GIMBAL_UNRESET;
		gimbal_last_state = GIMBAL_UNRESET;
	}
	leso_6020_init(&pitch_imu_leso,0.2f, 5.0f,1.5f);
	pitch_imu_leso.af_z1 = 0.05f;
	leso_6020_init(&yaw_imu_leso,0.2f, 5.0f,1.5f);
	yaw_imu_leso.af_z1 = 0.05f;
	
	kalman1_init(&i_pitch_filter,0,0.1,60);
	kalman1_init(&i_yaw_filter,0,0.1,60);
	gimbal_reset_flag = 0;
}
float pitch_input, yaw_input;
void gimbal_control_loop(rc_ctrl_t* rc_recv){
	
	float vision_pitch,vision_yaw;
	
	uint8_t vision_checksum;
	vision_checksum = vision_ctrl(&vision_pitch, &vision_yaw, 0.f);

	if(get_motor_rx_flags(GIMBAL_MOTORS_MASKS) == GIMBAL_MOTORS_MASKS){
		//clear_motor_rx_flags(GIMBAL_MOTORS_MASKS);
		if(rc_recv->rc.switch_right == RC_SW_DOWN){

			start_time_cnt = 0;
			gimbal_state = GIMBAL_UNRESET;
			gimbal_last_state = GIMBAL_UNRESET;
			reset_chassis_angle();
			shut_motors_output(&(GIMBAL_MOTORS_HCAN),M6020_ID_BASE);
			gimbal_reset_flag = 0;
			
		}else if(rc_recv->rc.switch_right == RC_SW_MID || rc_recv->rc.switch_right == RC_SW_UP){
			motor_response_msg_t* motor_ptr = get_motor_data_ptr();
		
			
			if(gimbal_state != GIMBAL_UNRESET && gimbal_state != GIMBAL_RESETING){
				gimbal_reset_flag = 1;
//				yaw_ang_ref=motor_ptr[CAN_GM6020_YAW_ID].raw_angle;
//					yaw_ang_ref=0;
				if(!is_rc_offline()){
					
					int16_t yaw_err = (int16_t)_imu_eular.yaw - yaw_ang_ref;               
					if(yaw_err < -ANGLE_RANGE/2) yaw_err += ANGLE_RANGE;
					gimbal_command_data.yaw_err=yaw_err;
					
					if(rc_recv->rc.switch_right == RC_SW_UP){
						#if NOD_HEAD == 1
//						if (ttf_cnt<3000){//spinning
//							ttf_cnt++;
						if (pitch_ang_ref >= gimabal_inspect_pitch_max)
						{
							pitch_ang_ref = gimabal_inspect_pitch_max;
							pitch_sign_inspect = -1;
						}
						else if (pitch_ang_ref <= gimabal_inspect_pitch_min)
						{
							pitch_ang_ref = gimabal_inspect_pitch_min;
							pitch_sign_inspect = 1;
						}
						pitch_inspect_speed = RNG_Get_RandomRange(3,9);
						pitch_input = (int16_t)(pitch_sign_inspect * pitch_inspect_speed);
						yaw_input = vision_control_data1.Wz;
						//yaw_input = (int16_t)(yaw_sign_inspect * yaw_inspect_speed);
						#else
						if(pitch_state_change())
						{
							pitch_state++;
							pitch_state_cnt=0;
						}
						if(pitch_state == HIGH_PITCH)
						{
							pitch_ang_ref = -200;
						}
						else if(pitch_state == LOW_PITCH)
						{
							pitch_ang_ref = 350;
						}
						else if (pitch_state == MID_PITCH)
						{
							pitch_ang_ref = 125;						
						}
						else
						{
							pitch_ang_ref = 125;
							pitch_state=0;
						}
						#endif				
//						gimbal_command_data.time_to_spinning=1;
//						gimbal_command_data.time_to_stop=0;
//						gimbal_command_data.time_to_go_l=0;
//						gimbal_command_data.time_to_go_r=0;}
//						else if (ttf_cnt>=3000&&ttf_cnt<5000){//follow
//						ttf_cnt++;
//						pitch_input=0;
//						yaw_input=0;
//					  pitch_ang_ref=  write_down_demo_pitch;
//					  yaw_ang_ref= write_down_demo_yaw;
//						gimbal_command_data.time_to_spinning=0;
//						gimbal_command_data.time_to_stop=1;
//						gimbal_command_data.time_to_go_l=0;
//						gimbal_command_data.time_to_go_r=0;}
//						else if (ttf_cnt>=5000&&ttf_cnt<=6000){//turn right
//						ttf_cnt++;
//						pitch_input=0;
//						yaw_input=0;
//					  pitch_ang_ref=  write_down_demo_pitch;
//					  yaw_ang_ref= write_down_demo_yaw;
//						gimbal_command_data.time_to_spinning=0;
//						gimbal_command_data.time_to_stop=0;
//						gimbal_command_data.time_to_go_l=0;
//						gimbal_command_data.time_to_go_r=1;}
//						else if (ttf_cnt>6000&&ttf_cnt<=9000){//spinning
//						ttf_cnt++;
//						if (pitch_ang_ref >= gimabal_inspect_pitch_max)
//						{
//							pitch_ang_ref = gimabal_inspect_pitch_max;
//							pitch_sign_inspect = -1;
//						}
//						else if (pitch_ang_ref <= gimabal_inspect_pitch_min)
//						{
//							pitch_ang_ref = gimabal_inspect_pitch_min;
//							pitch_sign_inspect = 1;
//						}
//						pitch_inspect_speed = RNG_Get_RandomRange(3,9);
//						pitch_input = (int16_t)(pitch_sign_inspect * pitch_inspect_speed);
//						yaw_input = (int16_t)(yaw_sign_inspect * yaw_inspect_speed);
//						gimbal_command_data.time_to_spinning=1;
//						gimbal_command_data.time_to_stop=0;
//						gimbal_command_data.time_to_go_l=0;
//						gimbal_command_data.time_to_go_r=0;}
//						else if (ttf_cnt>=9000&&ttf_cnt<=11000){//follow
//						ttf_cnt++;
//						pitch_input=0;
//						yaw_input=0;
//					  pitch_ang_ref=  write_down_demo_pitch;
//					  yaw_ang_ref= write_down_demo_yaw;
//						gimbal_command_data.time_to_spinning=0;
//						gimbal_command_data.time_to_stop=1;
//						gimbal_command_data.time_to_go_l=0;
//						gimbal_command_data.time_to_go_r=0;}
//						else if (ttf_cnt>=11000&&ttf_cnt<=12000){//turn left
//						ttf_cnt++;
//						pitch_input=0;
//						yaw_input=0;
//					  pitch_ang_ref=  write_down_demo_pitch;
//					  yaw_ang_ref= write_down_demo_yaw;
//						gimbal_command_data.time_to_spinning=0;
//						gimbal_command_data.time_to_stop=0;
//						gimbal_command_data.time_to_go_l=1;
//						gimbal_command_data.time_to_go_r=0;}
//						else if (ttf_cnt>12000){//to begin
//						ttf_cnt =0;}
					}else{
						pitch_input = -(int16_t)((float)rc_recv->rc.ch3 * GIMBAL_MOVE_SPEED / 660);
						yaw_input = (int16_t)((float)rc_recv->rc.ch2 * GIMBAL_MOVE_SPEED / 660);
					}
					
					if(!((motor_ptr[CAN_GM6020_PITCH_ID].raw_angle < GIMBAL_PITCH_MIN && pitch_input > 0.0f) ||
						(motor_ptr[CAN_GM6020_PITCH_ID].raw_angle > GIMBAL_PITCH_MAX && pitch_input < 0.0f))){
							pitch_ang_ref += pitch_input;
					}else{
						pitch_input = 0;
					}
					yaw_ang_ref += yaw_input;
				}
				
				if(!is_vision_offline()){
						if (vision_checksum == VISION_OK)
					{

						pitch_ang_ref = -(int32_t)vision_pitch;
						yaw_ang_ref = (int32_t)vision_yaw;
					}
				}
				
				if(yaw_ang_ref < -ANGLE_RANGE/2) 
				{yaw_ang_ref += ANGLE_RANGE;}
				else if(yaw_ang_ref > ANGLE_RANGE/2) {yaw_ang_ref -= ANGLE_RANGE;}
				
				if(gimbal_state == GIMBAL_IMU_CTRL){
					 
					shoot_allow_control(yaw_ang_ref,_imu_eular.yaw);
					gimbal_imu_ctrl(&_imu_eular,&_real_imu,pitch_ang_ref, yaw_ang_ref);
					
				}else if(gimbal_state == GIMBAL_MOTOR_CTRL){
					LIMIT_MIN_MAX(pitch_ang_ref, GIMBAL_PITCH_MIN, GIMBAL_PITCH_MAX);
					gimbal_ctrl(motor_ptr,pitch_ang_ref, yaw_ang_ref);
				}else{
					if(gimbal_state == GIMBAL_RESETED){
						pitch_ang_ref = (int16_t)_imu_eular.pitch;
//						pitch_ang_ref = 0;
						yaw_ang_ref = (int16_t)_imu_eular.yaw;
//						write_down_demo_pitch = (int16_t)_imu_eular.pitch;
//						write_down_demo_yaw   = (int16_t)_imu_eular.yaw;
						gimbal_state = GIMBAL_IMU_CTRL;
					}
				}
			}else{
				gimbal_state = gimbal_smooth_reset(motor_ptr);
			}
			gimbal_last_state = gimbal_state;
		}else{
			shut_motors_output(&(GIMBAL_MOTORS_HCAN),M6020_ID_BASE);
		}
	}
	get_gimbal_reset_flag(gimbal_reset_flag);
//	shut_motors_output(&(GIMBAL_MOTORS_HCAN),M6020_ID_BASE);
}

uint32_t is_gimbal_reset(void){ return (gimbal_state != GIMBAL_UNRESET && gimbal_state != GIMBAL_RESETING); }

/*云台缓启动，使云台保持与底盘同向*/
gimbal_state_t gimbal_smooth_reset(motor_response_msg_t* motor_data_ptr){
	int16_t delta_ang_pitch,delta_ang_yaw;
	int16_t delta_ang_pitch_ref,delta_ang_yaw_ref;
	if(motor_data_ptr == NULL) return GIMBAL_RESETED;

	motor_response_msg_t pitch_motor = motor_data_ptr[CAN_GM6020_PITCH_ID];
	motor_response_msg_t yaw_motor = motor_data_ptr[CAN_GM6020_YAW_ID];

	if(gimbal_state == GIMBAL_RESETING){
		delta_ang_pitch_ref = get_delta_ang(GIMBAL_PITCH_CENTER,pitch_ang_ref);
		delta_ang_yaw_ref = get_delta_ang(GIMBAL_YAW_CENTER,yaw_ang_ref);

		pitch_ang_ref += CLAMP(delta_ang_pitch_ref,SOFT_START_SPEED);
		yaw_ang_ref += CLAMP(delta_ang_yaw_ref,SOFT_START_SPEED);

		gimbal_ctrl(motor_data_ptr, pitch_ang_ref, yaw_ang_ref);
		
		delta_ang_pitch = get_delta_ang(GIMBAL_PITCH_CENTER,pitch_motor.raw_angle);
		delta_ang_yaw = get_delta_ang(GIMBAL_YAW_CENTER,yaw_motor.raw_angle);
		
		if(start_time_cnt < SOFT_START_MAX_CNT){
			start_time_cnt++;
			if(abs(delta_ang_pitch) <= SOFT_START_DEADBAND 
				&& abs(delta_ang_yaw) <= SOFT_START_DEADBAND){
				TRICE( Id(33663),"MSG: Gimbal startup successfully\n");
				return GIMBAL_RESETED;
			}else return GIMBAL_RESETING;
		}else{
			TRICE( Id(57575),"MSG: Gimbal soft start timeout, skipped\n");
			return GIMBAL_RESETED;
		}
		//else return GIMBAL_RESETED;
	}else{
		pitch_ang_ref = pitch_motor.raw_angle;
		yaw_ang_ref = yaw_motor.raw_angle;
		return GIMBAL_RESETING;
	}
}

/*纯电机控制云台*/
HAL_StatusTypeDef gimbal_ctrl(motor_response_msg_t* motor_data_ptr
							,int16_t raw_pitch_ang, int16_t raw_yaw_ang){
	motor_transmit_msg_t tx_msg;
	int16_t delta_ang_pitch,delta_ang_yaw;
	int16_t current_yaw_angle;
	
	if(motor_data_ptr == NULL) return HAL_ERROR;

	motor_response_msg_t pitch_motor = motor_data_ptr[CAN_GM6020_PITCH_ID];
	motor_response_msg_t yaw_motor = motor_data_ptr[CAN_GM6020_YAW_ID];
	
	memset(tx_msg.D,0,sizeof(motor_transmit_msg_t));
	
	delta_ang_pitch = get_delta_ang(raw_pitch_ang,pitch_motor.raw_angle);
	
	if(gimbal_state == GIMBAL_MOTOR_CTRL){
		current_yaw_angle = get_gimbal_angle(motor_data_ptr,&motor_data_ptr[CAN_GM6020_YAW_ID]);
	}else{
		current_yaw_angle = yaw_motor.raw_angle;
	}
	delta_ang_yaw = get_delta_ang(raw_yaw_ang,current_yaw_angle);
	
	//CAN_GM6020_PITCH_ID start from 4 (id-0x201)
	float pitch_current_rpm = iir_filter_2(pitch_motor.speed_rpm, GIMBAL_PITCH_FILTER2_RPM_CH);
	//任意电机在发送数据包中的位置可以用原始ID对4求余获得
	tx_msg.D[((CAN_GM6020_PITCH_ID-CAN_1_2_DIV) % 0x04)] = (int16_t)pid_dual_loop(pitch_pid,delta_ang_pitch, pitch_current_rpm);
	
	set_motor_output(&(GIMBAL_PITCH_HCAN),&tx_msg,M6020_ID_BASE);
	memset(tx_msg.D,0,sizeof(motor_transmit_msg_t));
	//chassis follow gimbal stablize
	//directions: chassis w_z + ,yaw speed_rpm -, wz uses ground as reference system
	//Yaw轴旋转转速，以下情况为6020电机电源口在底盘上
	float yaw_current_rpm = iir_filter_2(yaw_motor.speed_rpm, GIMBAL_YAW_FILTER2_RPM_CH);
									//- get_current_chassis_wz();

	tx_msg.D[(CAN_GM6020_YAW_ID % 0x04)] = (int16_t)pid_dual_loop(yaw_pid, delta_ang_yaw, yaw_current_rpm);
	
	return set_motor_output(&(GIMBAL_YAW_HCAN),&tx_msg,M6020_ID_BASE);
	
}

/*IMU控制云台*/

eular_t a;
int16_t delta_ang_pitch,delta_ang_yaw;
float yaw_current_rpm =0;
HAL_StatusTypeDef gimbal_imu_ctrl(eular_t* imu_eular, imu_data_fp_t* imu_gyro,
						int16_t raw_pitch_ang, int16_t raw_yaw_ang){
	motor_transmit_msg_t tx_msg;
	a=*imu_eular;
	
	
	motor_response_msg_t* motor_data_ptr = get_motor_data_ptr();
	
	motor_response_msg_t yaw_motor = motor_data_ptr[CAN_GM6020_YAW_ID];
	motor_response_msg_t pitch_motor = motor_data_ptr[CAN_GM6020_PITCH_ID];
	
	if(imu_gyro == NULL || imu_eular == NULL) return HAL_ERROR;
	

	memset(tx_msg.D,0,sizeof(motor_transmit_msg_t));
	
	delta_ang_pitch = -get_delta_ang(raw_pitch_ang,(int16_t)imu_eular->pitch);
							
//	LIMIT_MIN_MAX(delta_ang_pitch, 
//			GIMBAL_PITCH_MIN - pitch_motor.raw_angle, 
//			GIMBAL_PITCH_MAX - pitch_motor.raw_angle);
							
	
	delta_ang_yaw = -get_delta_ang(raw_yaw_ang,(int16_t)imu_eular->yaw);
	
	kalman1_filter(&i_pitch_filter,pitch_motor.current + pitch_motor.speed_rpm/13.33f*(30000.0f/24.0f));
	kalman1_filter(&i_yaw_filter,yaw_motor.current + yaw_motor.speed_rpm/13.33f*(30000.0f/24.0f));
	
	
	//CAN_GM6020_PITCH_ID start from 4 (id-0x201)
	//float pitch_current_rpm = iir_filter_2(pitch_motor.speed_rpm, GIMBAL_PITCH_FILTER2_RPM_CH);
	float pitch_current_rpm = imu_gyro->gy*25.0f;
	
//	tx_msg.D[((CAN_GM6020_PITCH_ID-CAN_1_2_DIV) % 0x04)] =
//		 (int16_t)CLAMP(pid_dual_loop(pitch_imu_pid, delta_ang_pitch, pitch_current_rpm)+
//		 PITCH_COMP*cosf(imu_eular->pitch*(2*PI/ANGLE_RANGE)), GM6020_OUTPUT_MAX);

	
			pitch_output = (int16_t)CLAMP(pid_leso_dualloop(pitch_imu_pid,&pitch_imu_leso,delta_ang_pitch
			, pitch_current_rpm,i_pitch_filter.x) +
			PITCH_COMP*cosf(imu_eular->pitch*(2*PI/ANGLE_RANGE)), GM6020_OUTPUT_MAX);
			
			tx_msg.D[((CAN_GM6020_PITCH_ID-CAN_1_2_DIV) % 0x04)] = (int16_t)fir_filter_5(pitch_output,GIMBAL_PITCH_FIR_CH);
			
		set_motor_output(&(GIMBAL_PITCH_HCAN),&tx_msg,M6020_ID_BASE);           
		memset(tx_msg.D,0,sizeof(motor_transmit_msg_t));
	/*chassis follow gimbal stablize*/
	/*纯电机解算底盘*/
	//directions: chassis w_z + ,yaw speed_rpm -, wz uses ground as reference system
	//float yaw_current_rpm = iir_filter_2(yaw_motor.speed_rpm, GIMBAL_YAW_FILTER2_RPM_CH) - get_current_chassis_wz();;
	
	yaw_current_rpm = imu_gyro->gz*30.0f;
	//tx_msg.D[(CAN_GM6020_YAW_ID % 0x04)] = (int16_t)pid_dual_loop(yaw_imu_pid, delta_ang_yaw, yaw_current_rpm);

	 yaw_output	= (int16_t)pid_leso_dualloop(yaw_imu_pid,&yaw_imu_leso
		, delta_ang_yaw, yaw_current_rpm,i_yaw_filter.x);
	tx_msg.D[(CAN_GM6020_YAW_ID % 0x04)]  = (int16_t)fir_filter_5(yaw_output,GIMBAL_YAW_FIR_CH);
	//tx_msg.D[(CAN_GM6020_YAW_ID % 0x04)]=0;
	return set_motor_output(&(GIMBAL_YAW_HCAN),&tx_msg,M6020_ID_BASE);
	
}

void inc_pitch_state_cnt(void)
{
	if(pitch_state_cnt < PITCH_STATE_CNT_MAX) pitch_state_cnt++;
}
uint8_t pitch_state_change(void){ return pitch_state_cnt >= PITCH_STATE_CNT_MAX;}

